/*
 * VisionController.h
 *
 */

#ifndef VISIONCONTROLLER_H_
#define VISIONCONTROLLER_H_

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv/cxcore.h>
#include <iostream>

#include "../kinect/Kinect.h"
#include "../kinect/FreenectAdapter.h"
#include "./ObjectContext.h"
#include "./BallStrategy.h"
#include "./GoalStrategy.h"
#include "./ObstacleStrategy.h"
#include "./POI.h"
#include "../Config.h"

using namespace cv;
using namespace kinect;
using namespace std;
using namespace objpreproc;
using namespace utils;
using namespace poi;

namespace vctrl {

class VisionController {

private:

    Kinect* _kin;

    Mat* _bgr;
    Mat* _depth;

    Rect* _cut;

    Mat* _bgrProc;
    Mat* _depthProc;

    Mat* _mask;

    ObjectContext<BallStrategy>* _bctx;
    ObjectContext<GoalStrategy>* _gctx;
    ObjectContext<ObstacleStrategy>* _octx;

    vector<KeyPoint> _keypoints;
    vector<POI*> _pois;
    vector<POI*> _obstacles;
    POI* _ball;
    POI* _goal;

	pthread_mutex_t _mutex;

public:

	VisionController();

	~VisionController();

	void elaborateImage();

	//POI* findBall();

	//POI* findGoal();

	vector<POI*> findObstacles();

	//vector<POI*> getPOIs();

	void alza();

	void abbassa();

	void azzera();

	float getGoalDepth();

	float getBallDepth();

	float getGoalAngle();

	float getBallAngle();
};

} /* namespace vctrl */

#endif /* VISIONCONTROLLER_H_ */
